API Reference / Adding Algorithms
 |
|
Introduction
This section explains to the user how to add different type of algorithms when needed, specifically shows how to add Interpolator and Integrator algorithms.
Integrator
The integrator algorithms work with orientation data captured by the IMU sensor, defined as Quaternions. These algorithms estimate an object's orientation using gyroscope and accelerometer measurements for improved accuracy. An initial orientation is necessary to begin the estimation process. To add a new integrator algorithm, follow these steps:
- Define the Integrator Algorithm class, inheriting from the IIntegrator class.
- Add the new Integrator Algorithm to the Integrator Interface.
Define the Integrator Algorithm
The RidgeRun Integrator Interface, known as IIntegrator, is an extensible class design to support various types of integrators. To add a new integrator to it, a new integrator class that derives from the IIntegrator class must be implemented. This class shall implement the following methods:
- Apply: applies the integrator algorithm to the entry orientation data.
- Reset: resets the initial orientation and its initial base on a desired value.
Defining the Apply method
This method receives two parameters:
- softgyro: This parameter contains the result of integrating the initial orientation data. It includes a vector of orientation values represented as Quaternions with their corresponding timestamps.
- rawgyro: This parameter contains raw sensor data, which may include measurements from the gyroscope, accelerometer, and magnetometer.
RuntimeError ExampleIntegrator::Apply(
std::vector<std::pair<Quaternion<double>, uint64_t>>& softengyro,
const std::vector<SensorPayload> rawgyro) {
RuntimeError ret{};
/* Integrator algorithm logic */
return ret;
Defining the Reset method
This method receives an orientation value as parameter and a time value (in microseconds) as parameter and resets integration based on them.
RuntimeError ExampleIntegrator::Reset(
const Quaternion<double> initial_orient, const uint64_t initial_time) {
RuntimeError ret{};
/* Reset initial orientation and initial time logic */
return ret;
}
Add the Integrator Algorithm to IIntegrator
In order to add the new integrator algorithm class to the integrator interface follow these steps:
- Add the constructor method to the integrator algorithm header file.
- Extend the Integrator Algorithms enumeration with the new integration algorithm.
- Add the integrator algorithm to the IIntegrator Build method.
Add the constructor method
The constructor method receives the integrator settings parameter. These settings contain the enable gyroscope and the enable accelerometer members that define if the gyroscope or the accelerometer is available; the settings also contain an enable complementary member that enables the complementary filter.
class ExampleIntegrator : public IIntegrator {
public:
explicit ExampleIntegrator(
const std::shared_ptr<IntegratorSettings>& settings);
/* Rest of class */
}
Extend the Integrator Algorithms enumeration
To enable the IIntegrator interface to create an instance of the new sensor, it must be added to the Integrator Algorithms enumeration.
enum class IntegratorAlgorithms {
kSimpleComplementaryIntegrator = 0,
/* Add new integrator algorithm */
kExampleIntegrator = 0
};
Add Integrator Algorithm to IIntegrator Build method
Finally, the new integrator case must be added to the IIntegrator build method to allow the interface to instantiate the integrator algorithm.
std::shared_ptr<IIntegrator> IIntegrator::Build(
IntegratorAlgorithms impl,
const std::shared_ptr<IntegratorSettings>& settings) {
switch (impl) {
case IntegratorAlgorithms::kSimpleComplementaryIntegrator:
return std::make_shared<rvs::SimpleComplementaryIntegrator>(settings);
/* Add new integrator algorithm case */
case IntegratorAlgorithms::kExampleIntegrator:
return std::make_shared<rvs:kExampleIntegrator>(settings);
default:
return nullptr;
}
}
Interpolator
The interpolator algorithms adjust the timestamps of the IMU sensor measurements to align with the timestamps of the captured frames. This alignment is necessary because the IMU sensor time and the camera sensor time (or system internal time) may not match. Therefore, the system needs to adjust the measurements based on the frame capture time. To add a new interpolator algorithm, follow these steps:
- Define the Interpolator Algorithm class, inheriting from the IInterpolator class.
- Add the new Interpolator Algorithm to the Interpolator Interface.
Define the Interpolator Algorithm
The RidgeRun Interpolator Interface, known as IInterpolator, is an extensible class design to support various types of interpolators. To add a new interpolator to it, a new interpolator class that derives from the IInterpolator class must be implemented. This class must implement the following methods:
- Apply: applies the interpolator algorithm to the desired data based on an initial time.
- Reset: resets the initial time of the interpolator based on a desired value.
Defining the Apply method
This method receives two parameters:
- softgyro: This is the input data containing a vector of orientation values (Quaternions) along with their corresponding timestamps (in microseconds). It is used by the interpolation algorithm.
- interpolated: This contains a vector of interpolated orientation values with their corresponding interpolation timestamps (in microseconds).
- clean: This avoids cleaning or modifying the softgyro vector.
RuntimeError ExampleInterpolator::Apply(
std::vector<std::pair<Quaternion<double>, uint64_t>>&interpolated,
std::vector<std::pair<Quaternion<double>, uint64_t>>& softgyro,
const bool clean) {
RuntimeError ret{};
/* Interpolation algorithm logic */
return ret;
Defining the Reset method
This method resets the interpolation based on an start time value inserted as parameter.
RuntimeError ExampleInterpolator::Reset(const uint64_t start_time) {
RuntimeError ret{};
/* Reset start time logic */
return ret;
}
Add the Interpolator Algorithm to IInterpolator
In order to add the new interpolator algorithm class to the interpolator interface follow these steps:
- Add the constructor method to the interpolator algorithm header file.
- Extend the Interpolator Algorithms enumeration with the new interpolation algorithm.
- Add the interpolator algorithm to the IInterpolator Build method.
Add the constructor method
The constructor method receives the interpolator settings parameter. These settings include the interpolation interval, which is a constant time (in microseconds) that updates the initial time after each interpolation.
class ExampleInterpolator : public IInterpolator {
public:
explicit ExampleInterpolator(
const std::shared_ptr<InterpolatorSettings> settings);
/* Rest of class */
}
Extend the Interpolator Algorithms enumeration
To enable the IInterpolator interface to create an instance of the new sensor, it must be added to the InterpolatorAlgorithms enumeration.
enum class InterpolatorAlgorithms {
kSlerp = 0,
/* Add new interpolator algorithm */
kExampleInterpolator
};
Add Interpolation Algorithm to IInterpolator Build method
Finally, the new interpolator case must be added to the IInterpolator build method to allow the interface to instantiate the interpolator algorithm.
std::shared_ptr<IInterpolator> IInterpolator::Build(
const InterpolatorAlgorithms impl,
const std::shared_ptr<InterpolatorSettings> settings) {
switch (impl) {
case InterpolatorAlgorithms::kSlerp:
return std::make_shared<SlerpInterpolator>(settings);
/* Add new interpolation algorithm case */
case InterpolatorAlgorithms::kExampleInterpolator:
return std::make_shared<ExampleInterpolator>(settings);
default:
return nullptr;
}
}